//------------------------------------------------------------------------------ 
//  <copyright file="ManagedPlanarPoseEstimator.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#include "ManagedExtractPlanarPose.h"

namespace Microsoft {
    namespace Robotics {
        namespace Vision {
            namespace GroundTruthExtraction {

/// <summary>
/// Initializes a new instance of the MSRPlanarPoseEstimator class.
/// </summary>
/// <param name="pdlColorIntrinsicParams">Camera intrinsic parameters </param>
/// <param name="cbColorIntrinsicParams">Camera intrinsic parameters length, should be 5</param>
/// <param name="pdlColorRDistortionParams">Camera distortion parameters </param>
/// <param name="cbColorIntrinsicParams">Camera distortion parameters length, should be 5</param>
ManagedPlanarPoseEstimator::ManagedPlanarPoseEstimator(
    double* pdlColorIntrinsicParams,
    int cbColorIntrinsicParams,
    double* pdlColorRDistortionParams,
    int cbColorRDistortionParams)    
{
    this->m_fDisposed = false;
    this->m_planarPoseEstimator = new MSRPlanarPoseEstimator(
        pdlColorIntrinsicParams,
        cbColorIntrinsicParams,
        pdlColorRDistortionParams,
        cbColorRDistortionParams);
}
        
/// <summary>
/// Destroys an instance of the ManagedPlanarPoseEstimator class.
/// </summary>
ManagedPlanarPoseEstimator::~ManagedPlanarPoseEstimator()
{
    this->!ManagedPlanarPoseEstimator();
    this->m_fDisposed = true;
}

/// <summary>
/// Disposes an instance of the ManagedPlanarPoseEstimator class.
/// </summary>
ManagedPlanarPoseEstimator::!ManagedPlanarPoseEstimator()
{
    if (this->m_planarPoseEstimator != nullptr)
    {
        delete this->m_planarPoseEstimator;
        this->m_planarPoseEstimator = nullptr;
    }
}

/// <summary>
/// Estimate planar translation and rotation for the given image
/// Rotation is output as (Euler) angles.
/// </summary>
/// <param name="image">The image in Gray 8 format </param>
/// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
/// <param name="cbTranslation">The length of translation buffer, should be 3</param>
/// <param name="pdlRotation">The rotation in x, y, z directions of the planar checker board in radians</param>
/// <param name="cbRotation">The length of rotation array, should be 3</param>
/// <returns>True if the estimation was successful, False otherwise</returns>
bool ManagedPlanarPoseEstimator::EstimatePose(
	ImageFrameGray8^ image,    
    double* pdlTranslation,
    DWORD cbTranslation,
    double* pdlRotation,
    DWORD cbRotation)
{         
    if (image && image->ImageData && pdlTranslation && pdlRotation)
    {
		pin_ptr<BYTE> p = &image->ImageData[0];
        return this->m_planarPoseEstimator->EsimatePlanarPose(
			(BYTE*)p,
			image->ImageData->Length,
			image->Width,
			image->Height,
            pdlTranslation,
            cbTranslation,
            pdlRotation,
            cbRotation);
    }
    else
    {
        return false;
    }
}

/// <summary>
/// Estimate planar translation and rotation for the given image
/// Rotation is output as a unit quaternion.
/// </summary>
/// <param name="image">The image in Gray 8 format </param>
/// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
/// <param name="cbTranslation">The length of translation buffer, should be 3</param>
/// <param name="pdlQuaternion">The rotation of the planar checker board, specified as a unit quaternion</param>
/// <param name="cbQuaternion">The length of rotation array, should be 4</param>
/// <returns>True if the estimation was successful, False otherwise</returns>
bool ManagedPlanarPoseEstimator::EstimatePoseWithRotationAsUnitQuaternion(
	ImageFrameGray8^ image,    
    double* pdlTranslation,
    DWORD cbTranslation,
    double* pdlQuaternion,
    DWORD cbQuaternion)
{         
    if (image && image->ImageData && pdlTranslation && pdlQuaternion)
    {
		pin_ptr<BYTE> p = &image->ImageData[0];
        return this->m_planarPoseEstimator->EsimatePlanarPoseWithRotationAsUnitQuaternion(
			(BYTE*)p,
			image->ImageData->Length,
			image->Width,
			image->Height,
            pdlTranslation,
            cbTranslation,
            pdlQuaternion,
            cbQuaternion);
    }
    else
    {
        return false;
    }
}
}}}}
